#include "pointcloud_filter.h"
#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/crop_hull.h>
#include <pcl/surface/concave_hull.h>
#include <pcl/filters/project_inliers.h>

PointCloudFilter::PointCloudFilter(){

}

// 过滤轴数据 [a,b]
void PointCloudFilter::filterAxis(pcl::PointCloud<pcl::PointXYZ>::Ptr& data_input, string axis, double a, double b){
    pcl::PassThrough<pcl::PointXYZ> pass;
    pass.setInputCloud(data_input);
    pass.setFilterFieldName(axis);  // 指定过滤的轴
    pass.setFilterLimits(a, b);  // 设置过滤范围
    pass.setNegative(true);  // 指定是否过滤掉符合条件的点
    pass.filter(*data_input);  // 应用滤波器    
}

// 体素网格滤波器（VoxelGrid Filter） 降采样
void PointCloudFilter::voxelFilter(pcl::PointCloud<pcl::PointXYZ>::Ptr& data_input, 
    double size_x, double size_y, double size_z){
    pcl::VoxelGrid<pcl::PointXYZ> voxel_filter;
    voxel_filter.setInputCloud(data_input);
    voxel_filter.setLeafSize(size_x, size_y, size_z); // 设置体素大小（单位：米）
    voxel_filter.filter(*data_input);
}

// 过滤机身数据 凸多边形
void PointCloudFilter::PolygonFilter(pcl::PointCloud<pcl::PointXYZ>::Ptr& data_input, 
    vector<pair<double, double>> polygon){
    // 构造轮廓点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr boundingbox_ptr (new pcl::PointCloud<pcl::PointXYZ>);
    for(auto point : polygon){
        boundingbox_ptr->push_back(pcl::PointXYZ(point.first, point.second, 0));
    }

	pcl::ConvexHull<pcl::PointXYZ> hull;	//创建凸包对象
	hull.setInputCloud(boundingbox_ptr);	//设置输入点云
	hull.setDimension(2);					//设置凸包维度
	std::vector<pcl::Vertices> polygons;	//设置 pcl::Vertice 类型的向量，用于保存凸包顶点
	//该点云用于描述凸包形状
	pcl::PointCloud<pcl::PointXYZ>::Ptr surface_hull (new pcl::PointCloud<pcl::PointXYZ>);
	hull.reconstruct(*surface_hull, polygons);		//计算 2D 凸包结果

	pcl::CropHull<pcl::PointXYZ> bb_filter;		//创建 CropHull 对象
	bb_filter.setDim(2);						//设置维度
	bb_filter.setInputCloud(data_input);		//设置输入需要滤波的点云
	bb_filter.setHullIndices(polygons);			//输入封闭多边形的顶点
	bb_filter.setHullCloud(surface_hull);		//输入封闭多边形的形状
    bb_filter.setCropOutside(false);
	bb_filter.filter(*data_input);				//执行 CropHull 滤波，结果存储到 objects
}

//  点云投影到XY平面
void PointCloudFilter::projectionXY(pcl::PointCloud<pcl::PointXYZ>::Ptr& data_input){
    pcl::ProjectInliers<pcl::PointXYZ> proj;
    proj.setModelType(pcl::SACMODEL_PLANE);
    proj.setInputCloud(data_input);
    pcl::ModelCoefficients::Ptr coeffs(new pcl::ModelCoefficients);
    coeffs->values.resize(4);
    coeffs->values[0] = 0;  // 平面方程 ax+by+cz+d=0
    coeffs->values[1] = 0;  // 这里设置z=0平面（XY平面）
    coeffs->values[2] = 1;
    coeffs->values[3] = 0;
    proj.setModelCoefficients(coeffs);
    proj.filter(*data_input);
}
